﻿#ifndef KEYFRAME_H_
#define KEYFRAME_H_
#include <queue>
#include <Eigen/Core>
#include <memory>
#include "common/nav_state.h"
#include "common/imu.h"
#include "common/gnss.h"

#include "ch4/imu_preintegration.h"

namespace sad {

class KeyFrame {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    KeyFrame(const std::shared_ptr<NavStated> this_frame, const std::shared_ptr<IMUPreintegration> pre_integ, const GNSS& this_gnss);
    ~KeyFrame();

    std::shared_ptr<NavStated> this_frame() {
        return this_frame_;
    }
    std::shared_ptr<IMUPreintegration> GetPreintegration() {
        return pre_integ_;
    }
    GNSS this_gnss() {
        return this_gnss_;
    }

    void SetImuState(const std::shared_ptr<NavStated>& imu_state);
    void SetImuBias(const Eigen::Vector3d &bg, const Eigen::Vector3d &ba);

    static unsigned int next_id_;
    unsigned int id_;
private:
    std::shared_ptr<NavStated> this_frame_ = nullptr;
    std::shared_ptr<IMUPreintegration> pre_integ_ = nullptr;

    GNSS this_gnss_;
};

}  // namespace sad

#endif  // KEYFRAME_H_
